// Copyright 2025 Enactic, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <atomic>
#include <chrono>
#include <csignal>
#include <iostream>
#include <openarm/can/socket/openarm.hpp>
#include <openarm/eyou_motor/eyou_motor_constants.hpp>
#include <thread>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>

enum operation_mode {
    OPER_MIN = 0,
    OPER_ENABLE,
    OPER_QUERY_PARAM,
    OPER_CONTROL,
    OPER_CONTROL_GRIB,
    OPER_DISABLE,
    OPER_MAX
};


int run() {
    try {
        std::cout << "=== OpenArm CAN Example ===" << std::endl;
        std::cout << "This example demonstrates the OpenArm API functionality" << std::endl;

        // Initialize OpenArm with CAN interface and enable CAN-FD
        std::cout << "Initializing OpenArm CAN..." << std::endl;
        openarm::can::socket::OpenArm openarm("vcan0", true);  // Use CAN-FD on can0 interface

        // Initialize arm motors
        std::vector<openarm::eyou_motor::MotorType> motor_types = {
            openarm::eyou_motor::MotorType::DM4310, openarm::eyou_motor::MotorType::DM4310};
        std::vector<uint32_t> send_can_ids = {0x01, 0x02};
        std::vector<uint32_t> recv_can_ids = {0x11, 0x12};

        openarm.init_arm_motors(motor_types, send_can_ids, recv_can_ids);

        // Initialize gripper
        std::cout << "Initializing gripper..." << std::endl;
        openarm.init_gripper_motor(openarm::eyou_motor::MotorType::DM4310, 0x08, 0x18);

        // Set callback mode to ignore and enable all motors
        openarm.set_callback_mode_all(openarm::eyou_motor::CallbackMode::IGNORE);

        int op = 0;

        while(1)
        {
          std::cout << "\n**************Please input operation_mode***************" << std::endl;
          std::cout << "OPER_ENABLE: "         << OPER_ENABLE << std::endl;
          std::cout << "OPER_QUERY_PARAM: "    << OPER_QUERY_PARAM << std::endl;
          std::cout << "OPER_CONTROL: "        << OPER_CONTROL << std::endl;
          std::cout << "OPER_CONTROL_GRIB: "   << OPER_CONTROL_GRIB << std::endl;
          std::cout << "OPER_DISABLE: "        << OPER_DISABLE << std::endl;

          std::cin >> op;
          if(op == 999) break;

          operation_mode op_mode = static_cast<operation_mode>(op);
          std::cout << "input op=" << op_mode << std::endl;
          switch (op_mode)
          {
          case OPER_ENABLE:
              std::cout << "\n=== Enabling Motors ===" << std::endl;
              openarm.enable_all();
              openarm.recv_all(2000);
              break;
          case OPER_QUERY_PARAM:
              // Set device mode to param and query motor id
              std::cout << "\n=== Querying Motor Recv IDs ===" << std::endl;
              openarm.set_callback_mode_all(openarm::eyou_motor::CallbackMode::PARAM);
              openarm.query_param_all(static_cast<int>(openarm::eyou_motor::RID::MST_ID));
              // Allow time (2ms) for the motors to respond for slow operations like querying
              // parameter from register
              openarm.recv_all(2000);
              break;
          case OPER_CONTROL:
              // Set device mode to state and control motor
              std::cout << "\n=== Controlling Motors ===" << std::endl;
              openarm.set_callback_mode_all(openarm::eyou_motor::CallbackMode::STATE);

              // Control arm motors with position control
              openarm.get_arm().mit_control_all({openarm::eyou_motor::MITParam{2, 1, 0, 0, 0},
                                                 openarm::eyou_motor::MITParam{2, 1, 0, 0, 0}});
              openarm.recv_all(500);

              // Control arm motors with torque control
              openarm.get_arm().mit_control_all({openarm::eyou_motor::MITParam{0, 0, 0, 0, 0.1},
                                                 openarm::eyou_motor::MITParam{0, 0, 0, 0, 0.1}});
              openarm.recv_all(500);
              break;
          case OPER_CONTROL_GRIB:
            // Control gripper
            std::cout << "Closing gripper..." << std::endl;
            openarm.get_gripper().close();
            openarm.recv_all(1000);
            break;
          case OPER_DISABLE:
            openarm.disable_all();
            openarm.recv_all(1000);
            break;
          default:
              break;
          }


          // Access motors through components
          if(0)
          {
              for (const auto& motor : openarm.get_arm().get_motors()) {
                  std::cout << "Arm Motor: " << motor.get_send_can_id() << " ID: "
                          << motor.get_param(static_cast<int>(openarm::eyou_motor::RID::MST_ID))
                          << std::endl;
              }
              for (const auto& motor : openarm.get_gripper().get_motors()) {
                  std::cout << "Gripper Motor: " << motor.get_send_can_id() << " ID: "
                          << motor.get_param(static_cast<int>(openarm::eyou_motor::RID::MST_ID))
                          << std::endl;
              }
          }


          if(0)
          {
              for (int i = 0; i < 10; i++) {
                  std::this_thread::sleep_for(std::chrono::milliseconds(100));

                  openarm.refresh_all();
                  openarm.recv_all(300);

                  // Display arm motor states
                  for (const auto& motor : openarm.get_arm().get_motors()) {
                      std::cout << "Arm Motor: " << motor.get_send_can_id()
                              << " position: " << motor.get_position() << std::endl;
                  }
                  // Display gripper state
                  for (const auto& motor : openarm.get_gripper().get_motors()) {
                      std::cout << "Gripper Motor: " << motor.get_send_can_id()
                              << " position: " << motor.get_position() << std::endl;
                  }
              }
          }
        }


    } catch (const std::exception& e) {
        std::cerr << "Error: " << e.what() << std::endl;
        return -1;
    }

    return 0;
}

int main()
{
    run();
    return 0;
}

